#include <OpenGLES/ES1/gl.h>
#include <OpenGLES/ES1/glext.h>

#include <vector>
#include <cmath>
#include <cassert>

#include "AABB.h"
//#include "global_includes.h" //TODO: Check dependencies

#include "Plane.h"
#include "Matrix33.h"
#include "Matrix44.h"
//#include "simplePrimitives.h"  //TODO: Check dependencies

using namespace gti;

unsigned int AABB::corners_table[8][3] =
{
	{0, 0, 0},
	{0, 0, 1},
	{0, 1, 0},
	{0, 1, 1},
	{1, 0, 0},
	{1, 0, 1},
	{1, 1, 0},
	{1, 1, 1}
};

unsigned int AABB::edge_indices[12][2] = 
{
	{0, 1},
	{2, 3},
	{4, 5},
	{6, 7},
	{0, 2},
	{1, 3},
	{4, 6},
	{5, 7},
	{0, 4},
	{1, 5},
	{2, 6},
	{3, 7}
};

unsigned int AABB::edge_halfsize[12] = 
{
	2,
	2,
	2,
	2,
	1,
	1,
	1,
	1,
	0,
	0,
	0,
	0
};

vector3f AABB::edge_dir[12] = 
{
	vector3f(0,0,1),
	vector3f(0,0,1),
	vector3f(0,0,1),
	vector3f(0,0,1),
	vector3f(0,1,0),
	vector3f(0,1,0),
	vector3f(0,1,0),
	vector3f(0,1,0),
	vector3f(1,0,0),
	vector3f(1,0,0),
	vector3f(1,0,0),
	vector3f(1,0,0)
};

float AABB::plane_normals[6][3] =
{
	{ 1, 0, 0},
	{ 0, 1, 0},
	{ 0, 0, 1},
	{-1, 0, 0},
	{ 0,-1, 0},
	{ 0, 0,-1}
};

//CCW
unsigned int AABB::quad_indices[6][4] =
{
	{7, 5, 4, 6},
	{2, 3, 7, 6},
	{3, 1, 5, 7},
	{2, 0, 1, 3},
	{1, 0, 4, 5},
	{0, 2, 6, 4}
};

AABB::AABB()
{
}

AABB::AABB(const vector3f& center, const vector3f& halfsize) :
center(center),
halfsize(halfsize)
{
}

AABB::AABB(float cx, float cy, float cz, float hx, float hy, float hz) :
center(cx,cy,cz),
halfsize(hx,hy,hz)
{
}

void AABB::set(const vector3f& acenter, const vector3f& ahalfsize)
{
	center=acenter;
	halfsize=ahalfsize;
}

void AABB::set(const vector3f& acenter, float hs)
{
	center=acenter;
	halfsize.set(hs,hs,hs);
}

void AABB::setMinMax(const vector3f& vmin, const vector3f& vmax)
{
	halfsize = (vmax-vmin)*0.5f;
	center = vmin + halfsize;
}

void AABB::expand(float size)
{
    halfsize *= size;
}

void AABB::extendBy(const vector3f& point)
{
	vector3f vmin = center - halfsize;
	vector3f vmax = center + halfsize;

	int i;
	for (i=0;i<3;i++)
	{
		if (vmin[i] > point[i]) vmin[i] = point[i];
		else if (vmax[i] < point[i]) vmax[i] = point[i];
	}

	setMinMax(vmin,vmax);
}

void AABB::extendBy(const AABB& aabb)
{
	vector3f avmin = center - halfsize;
	vector3f avmax = center + halfsize;
	const vector3f & bvmin = aabb.center - aabb.halfsize;
	const vector3f & bvmax = aabb.center + aabb.halfsize;

	int i;
	for (i=0;i<3;i++)
	{
		if (avmin[i] > bvmin[i]) avmin[i] = bvmin[i];
		if (avmax[i] < bvmax[i]) avmax[i] = bvmax[i];
	}

	setMinMax(avmin,avmax);
}

void AABB::calcule(const std::vector<vector3f>& positions)
{
	vector3f vmin = positions[0];
	vector3f vmax = positions[0];

	for (unsigned int i = 1; i < positions.size(); ++i)
	{
		if (vmin.x > positions[i].x) vmin.x = positions[i].x;
		else if (vmax.x < positions[i].x) vmax.x = positions[i].x;
		if (vmin.y > positions[i].y) vmin.y = positions[i].y;
		else if (vmax.y < positions[i].y) vmax.y = positions[i].y;
		if (vmin.z > positions[i].z) vmin.z = positions[i].z;
		else if (vmax.z < positions[i].z) vmax.z = positions[i].z;
	}
	setMinMax(vmin,vmax);
}

void AABB::transform(const matrix44& mat, AABB& newAABB) const
{
	unsigned int j,k;
	const vector3f points[2] = 
	{
		center - halfsize,
		center + halfsize
	};

	vector3f vmin,vmax, corner;

	// initialize the corners to the transformed center of the aabb
	// TODO: this can be avoided initializing the vmin and vmax to
	// the first transformed corner of the new aabb
	vmin = mat * center;
	vmax = vmin;

	//transform all the corners and include them into the new box.
	for (j=0;j<8;j++)
	{
		// TODO: test if a precomputed index table is faster than binary method
		//Find all corners the "binary" way :-)
		corner.set(points[(j&4)>>2][0], points[(j&2)>>1][1], points[j&1][2]);
		const vector3f & vtx = mat*corner;

		for (k=0;k<3;k++)
		{
			if (vmin[k] > vtx[k]) vmin[k] = vtx[k];
			else if (vmax[k] < vtx[k]) vmax[k] = vtx[k];
		}
	}

	newAABB.setMinMax(vmin,vmax);
}

void AABB::getMin(vector3f& vmin) const
{
	vmin.x = center.x - halfsize.x;
	vmin.y = center.y - halfsize.y;
	vmin.z = center.z - halfsize.z;
}

void AABB::getMax(vector3f& vmax) const
{
	vmax.x = center.x + halfsize.x;
	vmax.y = center.y + halfsize.y;
	vmax.z = center.z + halfsize.z;
}

void AABB::getMinMax(vector3f& vmin, vector3f& vmax) const
{
	vmin.x = center.x - halfsize.x;
	vmin.y = center.y - halfsize.y;
	vmin.z = center.z - halfsize.z;

	vmax.x = center.x + halfsize.x;
	vmax.y = center.y + halfsize.y;
	vmax.z = center.z + halfsize.z;
}

void AABB::getCorners(vector3f* corners) const
{
	vector3f points[2];
	getMin(points[0]);
	getMax(points[1]);

	for (unsigned int j=0;j<8;j++)
	{
		// the tests say that precomputed index table is faster than binary method
		//Find all corners the "binary" way :-)
		//corners[j].set(points[(j&4)>>2][0], points[(j&2)>>1][1], points[j&1][2]);
		// precomputed table method
		corners[j].set(
			points[corners_table[j][0]][0],
			points[corners_table[j][1]][1],
			points[corners_table[j][2]][2]
			);
	}
}

void AABB::getCorner(CornerId id, vector3f& corner) const
{
	vector3f points[2];
	getMin(points[0]);
	getMax(points[1]);

	//Find all corners the "binary" way :-)
	//corner.set(points[(id&4)>>2][0], points[(id&2)>>1][1], points[id&1][2]);
	// precomputed table method
	corner.set(
			points[corners_table[id][0]][0],
			points[corners_table[id][1]][1],
			points[corners_table[id][2]][2]
			);
}

void AABB::getRelativeCorner(const vector3f& point, vector3f& corner) const
{
	if (point.x>0) corner.x = center.x+halfsize.x;
	else corner.x = center.x-halfsize.x;
	if (point.y>0) corner.y = center.y+halfsize.y;
	else corner.y = center.y-halfsize.y;
	if (point.z>0) corner.z = center.z+halfsize.z;
	else corner.z = center.z-halfsize.z;
}

void AABB::getNearestCorner(const vector3f& point, vector3f& corner) const
{
	if (point.x>center.x) corner.x = center.x+halfsize.x;
	else corner.x = center.x-halfsize.x;
	if (point.y>center.y) corner.y = center.y+halfsize.y;
	else corner.y = center.y-halfsize.y;
	if (point.z>center.z) corner.z = center.z+halfsize.z;
	else corner.z = center.z-halfsize.z;
}

void AABB::getFurthestCorner(const vector3f& point, vector3f& corner) const
{
	if (point.x>center.x) corner.x = center.x-halfsize.x;
	else corner.x = center.x+halfsize.x;
	if (point.y>center.y) corner.y = center.y-halfsize.y;
	else corner.y = center.y+halfsize.y;
	if (point.z>center.z) corner.z = center.z-halfsize.z;
	else corner.z = center.z+halfsize.z;
}

void AABB::getPlanes(Plane* planes) const 
{
	// TO DO; AABB::getPlanes() must be unrolled 3+3
	for (unsigned int i=0;i<6;i++)
	{
		getPlane((PlaneId)i,planes[i]);
	}
}

void AABB::getPlane(PlaneId id, Plane& plane) const
{
	plane.n.set(
		plane_normals[id][0],
		plane_normals[id][1],
		plane_normals[id][2]);

	vector3f corner;
	if (id < LEFT) getMax(corner);
	else getMin(corner);

	// TO DO: convert vector dotproduct into a single scalar product
	plane.d = -DotProduct(plane.n,corner);
}

void AABB::render() const
{
/*
	static std::vector<vector3f> points;
	points.resize(12*3);

#ifdef USE_DIRECT3D
	// front
	points.push_back(vector3f(1,-1, -1));
	points.push_back(vector3f(1, 1, -1));
	points.push_back(vector3f(-1, 1,-1));

	points.push_back(vector3f(-1, 1,-1));
	points.push_back(vector3f(-1,-1, -1));
	points.push_back(vector3f(1,-1, -1));

	// back
	points.push_back(vector3f(1, 1, 1));
	points.push_back(vector3f(1,-1, 1));
	points.push_back(vector3f(-1, 1,1));

	
	points.push_back(vector3f(-1,-1, 1));
	points.push_back(vector3f(-1, 1,1));
	points.push_back(vector3f(1,-1, 1));

	// top
	points.push_back(vector3f(1,1, -1));
	points.push_back(vector3f(1, 1, 1));
	points.push_back(vector3f(-1, 1,1));

	points.push_back(vector3f(-1, 1,1));
	points.push_back(vector3f(-1,1,-1));
	points.push_back(vector3f(1,1, -1));

	// bottom
	points.push_back(vector3f(1, -1, 1));
	points.push_back(vector3f(1,-1, -1));
	points.push_back(vector3f(-1, -1,1));

	points.push_back(vector3f(-1,-1,-1));
	points.push_back(vector3f(-1, -1,1));
	points.push_back(vector3f(1,-1, -1));

	//left
	points.push_back(vector3f(-1, -1, 1));
	points.push_back(vector3f(-1, -1, -1));
	points.push_back(vector3f(-1, 1, 1));

	points.push_back(vector3f(-1, 1, -1));
	points.push_back(vector3f(-1, 1, 1));
	points.push_back(vector3f(-1, -1, -1));

	// right
	points.push_back(vector3f(1, -1, -1));
	points.push_back(vector3f(1, -1, 1));
	points.push_back(vector3f(1, 1, 1));

	points.push_back(vector3f(1, 1, 1));
	points.push_back(vector3f(1, 1, -1));
	points.push_back(vector3f(1, -1, -1));

#endif

#ifdef USE_OPENGL
	// front
	points[0].set(-1, 1,-1);
	points[1].set(1, 1, -1);
	points[2].set(1,-1, -1);
	
	points[3].set(1,-1, -1);
	points[4].set(-1,-1, -1);
	points[5].set(-1, 1,-1);
	
	// back
	points[6].set(-1, 1,1);
	points[7].set(1,-1, 1);
	points[8].set(1, 1, 1);
	
	points[9].set(1,-1, 1);
	points[10].set(-1, 1,1);
	points[11].set(-1,-1, 1);
	
	// top
	points[12].set(-1, 1,1);
	points[13].set(1, 1, 1);
	points[14].set(1,1, -1);
	
	points[15].set(1,1, -1);
	points[16].set(-1,1,-1);
	points[17].set(-1, 1,1);
		
	// bottom
	points[18].set(-1, -1,1);
	points[19].set(1,-1, -1);
	points[20].set(1, -1, 1);
	
	points[21].set(1,-1, -1);
	points[22].set(-1, -1,1);
	points[23].set(-1,-1,-1);
	
	//left
	points[24].set(-1, 1, 1);
	points[25].set(-1, -1, -1);
	points[26].set(-1, -1, 1);
	
	points[27].set(-1, -1, -1);
	points[28].set(-1, 1, 1);
	points[29].set(-1, 1, -1);
	
	// right
	points[30].set(1, 1, 1);
	points[31].set(1, -1, 1);
	points[32].set(1, -1, -1);
	
	points[33].set(1, -1, -1);
	points[34].set(1, 1, -1);
	points[35].set(1, 1, 1);
	
#endif
	
	for (int i=0;i<points.size(); i++)
	{
		points[i] = ScalarProduct(points[i],halfsize) + center;
	}
	renderTriangleList(points,vector3f(1,0,0));
*/
	
	/*
	{// RIGHT
		{-1,-1,-1},
		{-1, 1,-1},
		{-1, 1, 1},
		{-1,-1, 1},
	},
	{//TOP
		{-1, 1, 1},
		{-1, 1,-1},
		{ 1, 1,-1},
		{ 1, 1, 1},
	},
	{//BOTTOM
		{ 1,-1,-1},
		{-1,-1,-1},
		{-1,-1, 1},
		{ 1,-1, 1},
	},
	{//BACK
		{-1,-1, 1},
		{-1, 1, 1},
		{ 1, 1, 1},
		{ 1,-1, 1},
	},
	{//FRONT
		{ 1,-1,-1},
		{ 1, 1,-1},
		{-1, 1,-1},
		{-1,-1,-1},
*/
///*
/*	glPushMatrix();

	glTranslatef(center.x,center.y,center.z);

	float lx = halfsize[0];
	float ly = halfsize[1];
	float lz = halfsize[2];

	// sides
	glBegin (GL_TRIANGLE_STRIP);
	glNormal3f (-1,0,0);
	glVertex3f (-lx,-ly,-lz);
	glVertex3f (-lx,-ly,lz);
	glVertex3f (-lx,ly,-lz);
	glVertex3f (-lx,ly,lz);
	glNormal3f (0,1,0);
	glVertex3f (lx,ly,-lz);
	glVertex3f (lx,ly,lz);
	glNormal3f (1,0,0);
	glVertex3f (lx,-ly,-lz);
	glVertex3f (lx,-ly,lz);
	glNormal3f (0,-1,0);
	glVertex3f (-lx,-ly,-lz);
	glVertex3f (-lx,-ly,lz);
	glEnd();

	// top face
	glBegin (GL_TRIANGLE_FAN);
	glNormal3f (0,0,1);
	glVertex3f (-lx,-ly,lz);
	glVertex3f (lx,-ly,lz);
	glVertex3f (lx,ly,lz);
	glVertex3f (-lx,ly,lz);
	glEnd();

	// bottom face
	glBegin (GL_TRIANGLE_FAN);
	glNormal3f (0,0,-1);
	glVertex3f (-lx,-ly,-lz);
	glVertex3f (-lx,ly,-lz);
	glVertex3f (lx,ly,-lz);
	glVertex3f (lx,-ly,-lz);
	glEnd();

	glPopMatrix();*/
//*/
}

void AABB::renderWire(const matrix44& vp) const
{
	static std::vector<gti::vector3f> vertices;
	vertices.resize(12*2);

	gti::vector3f corners[8];
	getCorners(corners);

	for (int i=0;i<12;i++)
	{
		vertices[i*2] = (corners[edge_indices[i][0]]);
		vertices[i*2+1] = (corners[edge_indices[i][1]]);
	}

	//Renderer::renderLineList(mvp, vertices);
	/*glBegin(GL_LINES);
	for (int i=0;i<(int)(vertices.size())-1;i+=2)
	{
		glVertex3fv(vertices[i]);
		glVertex3fv(vertices[i+1]);
	}
	glEnd();*/
	static GLfloat v_array[72];
	//glMatrixMode(GL_MODELVIEW);
	glEnableClientState(GL_VERTEX_ARRAY);
	assert(glGetError() == GL_NO_ERROR);
	
	int i=0;
	for (int j=0;j<24;j++)
	{
		v_array[i] = vertices[j][0];
		v_array[i+1] = vertices[j][1];
		v_array[i+2] = vertices[j][2];
		i+=3;
	}
	glVertexPointer(3, GL_FLOAT, 0, &vertices[0]);
	assert(glGetError() == GL_NO_ERROR);
	
	glDrawArrays(GL_LINES, 0, 24);
	assert(glGetError() == GL_NO_ERROR);
}


#define X 0
#define Y 1
#define Z 2

#define FINDMINMAX(x0,x1,x2,min,max) \
  min = max = x0;   \
  if(x1<min) min=x1;\
  if(x1>max) max=x1;\
  if(x2<min) min=x2;\
  if(x2>max) max=x2;

/*======================== X-tests ========================*/
#define AXISTEST_X01(a, b, fa, fb)			   \
	p0 = a*v0[Y] - b*v0[Z];			       	   \
	p2 = a*v2[Y] - b*v2[Z];			       	   \
        if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
	rad = fa * halfsize[Y] + fb * halfsize[Z];   \
	if(min>rad || max<-rad) return false;

#define AXISTEST_X2(a, b, fa, fb)			   \
	p0 = a*v0[Y] - b*v0[Z];			           \
	p1 = a*v1[Y] - b*v1[Z];			       	   \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * halfsize[Y] + fb * halfsize[Z];   \
	if(min>rad || max<-rad) return false;

/*======================== Y-tests ========================*/
#define AXISTEST_Y02(a, b, fa, fb)			   \
	p0 = -a*v0[X] + b*v0[Z];		      	   \
	p2 = -a*v2[X] + b*v2[Z];	       	       	   \
        if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
	rad = fa * halfsize[X] + fb * halfsize[Z];   \
	if(min>rad || max<-rad) return false;

#define AXISTEST_Y1(a, b, fa, fb)			   \
	p0 = -a*v0[X] + b*v0[Z];		      	   \
	p1 = -a*v1[X] + b*v1[Z];	     	       	   \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * halfsize[X] + fb * halfsize[Z];   \
	if(min>rad || max<-rad) return false;

/*======================== Z-tests ========================*/

#define AXISTEST_Z12(a, b, fa, fb)			   \
	p1 = a*v1[X] - b*v1[Y];			           \
	p2 = a*v2[X] - b*v2[Y];			       	   \
        if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
	rad = fa * halfsize[X] + fb * halfsize[Y];   \
	if(min>rad || max<-rad) return false;

#define AXISTEST_Z0(a, b, fa, fb)			   \
	p0 = a*v0[X] - b*v0[Y];				   \
	p1 = a*v1[X] - b*v1[Y];			           \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * halfsize[X] + fb * halfsize[Y];   \
	if(min>rad || max<-rad) return false;


CullFlag AABB::overlap(const vector3f &vtx0,const vector3f &vtx1,const vector3f &vtx2) const
{
	if (collide(vtx0,vtx1,vtx2))
	{
		// check if the tree vertices are inside the aabb
		// TO DO: optimize aabb overlap test, unroll the 3 point.inside call
		if (contains(vtx0) &&contains(vtx1) && contains(vtx2))
		{
		//if (vtx0.inside(*this) && vtx1.inside(*this) && vtx2.inside(*this))
			return INSIDE;
		}
		return OVERLAP;
	}
	return OUTSIDE;
}


bool AABB::collide(const vector3f &vtx0,const vector3f &vtx1,const vector3f &vtx2) const
{
// AABB-triangle overlap test code
// by Tomas Akenine-Mˆller    
// http://www.acm.org/jgt/papers/AkenineMoller01/tribox.html

	/* move everything so that the boxcenter is in (0,0,0) */
	const vector3f& v0 = vtx0 - center;
	const vector3f& v1 = vtx1 - center;
	const vector3f& v2 = vtx2 - center;

	return collideTriangleBoxSpace(v0,v1,v2);
}

bool AABB::collideTriangleBoxSpace(const vector3f &v0,const vector3f &v1,const vector3f &v2) const
{
	float min,max,p0,p1,p2,rad,fex,fey,fez;
	/* compute triangle edges */

	const vector3f& e0 = v1-v0;      /* tri edge 0 */

	/* Bullet 3:  */
	/*  test the 9 tests first (this was faster) */
	fex = fabsf(e0[X]);
	fey = fabsf(e0[Y]);
	fez = fabsf(e0[Z]);

	AXISTEST_X01(e0[Z], e0[Y], fez, fey);
	AXISTEST_Y02(e0[Z], e0[X], fez, fex);
	AXISTEST_Z12(e0[Y], e0[X], fey, fex);

	const vector3f& e1 = v2-v1;      /* tri edge 1 */

	fex = fabsf(e1[X]);
	fey = fabsf(e1[Y]);
	fez = fabsf(e1[Z]);
	AXISTEST_X01(e1[Z], e1[Y], fez, fey);
	AXISTEST_Y02(e1[Z], e1[X], fez, fex);
	AXISTEST_Z0(e1[Y], e1[X], fey, fex);

	const vector3f& e2 = v0-v2;      /* tri edge 2 */

	fex = fabsf(e2[X]);
	fey = fabsf(e2[Y]);
	fez = fabsf(e2[Z]);
	AXISTEST_X2(e2[Z], e2[Y], fez, fey);
	AXISTEST_Y1(e2[Z], e2[X], fez, fex);
	AXISTEST_Z12(e2[Y], e2[X], fey, fex);

	/* Bullet 1: */
	/*  first test overlap in the {x,y,z}-directions */
	/*  find min, max of the triangle each direction, and test for overlap in */
	/*  that direction -- this is equivalent to testing a minimal AABB around */
	/*  the triangle against the AABB */

	/* test in X-direction */
	FINDMINMAX(v0[X],v1[X],v2[X],min,max);
	if(min>halfsize[X] || max<-halfsize[X]) return false;

	/* test in Y-direction */
	FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
	if(min>halfsize[Y] || max<-halfsize[Y]) return false;

	/* test in Z-direction */
	FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
	if(min>halfsize[Z] || max<-halfsize[Z]) return false;

	/* Bullet 2: */
	/*  test if the box intersects the plane of the triangle */
	/*  compute plane equation of triangle: normal*x+d=0 */

	Plane plane;
	plane.n = CrossProduct(e0,e1);
	plane.d=-DotProduct(plane.n,v0);  /* plane eq: normal.x+d=0 */

	AABB aabb;
	aabb.center.set(0,0,0);
	aabb.halfsize = halfsize;
	//if(plane.overlap(aabb)==OVERLAP)
	if(plane.overlap(aabb)!=INSIDE)
	{
		return true;
	}

//	assert(0 && "aabb triangle test odd situation");
	return false;
}

CullFlag AABB::overlap(const AABB& b) const
{
// test if the class overlap with a new aabb
// returns OVERLAP if the instance is overlapping b
// returns INSIDE  if the instance contains b
/*
	const float tx =  fabsf(b.center.x - center.x);
	const float ty =  fabsf(b.center.y - center.y);
	const float tz =  fabsf(b.center.z - center.z);
		
	if (tx <= (halfsize.x + b.halfsize.x) &&
		ty <= (halfsize.y + b.halfsize.y) &&
		tz <= (halfsize.z + b.halfsize.z) )
	{
		const vector3f& vmin = b.center-b.halfsize;
		if (vmin.inside(*this))
		{
			const vector3f& vmax = b.center+b.halfsize;
			if (vmax.inside(*this))
				return INSIDE;// this instance contains b
		}

		return OVERLAP; // this instance  overlaps with b
	}

	return OUTSIDE;
*/
	const float tx =  fabsf(b.center.x - center.x);		
	if (tx > (halfsize.x + b.halfsize.x))
		return OUTSIDE;
	const float ty =  fabsf(b.center.y - center.y);
	if (ty > (halfsize.y + b.halfsize.y))
		return OUTSIDE;
	const float tz =  fabsf(b.center.z - center.z);
	if (tz > (halfsize.z + b.halfsize.z) )
		return OUTSIDE;

	const vector3f& vmin = b.center-b.halfsize;
	if (contains(vmin))
	{
		const vector3f& vmax = b.center+b.halfsize;
		if (contains(vmax))
		{
			return INSIDE;// this instance contains b
		}
	}

	return OVERLAP; // this instance  overlaps with b
}

// fast aabb overlap test
bool AABB::collide(const AABB& b) const
{
/*
	const vector3f& T = b.center - center;//vector from A to B
	return (
		fabsf(T.x) <= (halfsize.x + b.halfsize.x) &&
		fabsf(T.y) <= (halfsize.y + b.halfsize.y) &&
		fabsf(T.z) <= (halfsize.z + b.halfsize.z) );
*/
	// optimized version
	const float x =  fabsf(b.center.x - center.x);
	if (x > (halfsize.x + b.halfsize.x)) return false;
	const float y =  fabsf(b.center.y - center.y);
	if (y > (halfsize.y + b.halfsize.y)) return false;
	const float z =  fabsf(b.center.z - center.z);
	if (z > (halfsize.z + b.halfsize.z)) return false;

	return true;
}



bool AABB::contains(const vector3f& point) const
{
	const vector3f& dif = center - point;
	return (
		fabsf(dif.x) < halfsize.x && 
		fabsf(dif.y) < halfsize.y &&
		fabsf(dif.z) < halfsize.z );
}


#undef FINDMINMAX
#undef AXISTEST_X01
#undef AXISTEST_Z12
#undef AXISTEST_X01
#undef AXISTEST_Y02
#undef AXISTEST_Z0
#undef AXISTEST_X2
#undef AXISTEST_Y1
#undef AXISTEST_Z12

// aabb sphere overlap test
CullFlag AABB::overlap (const gti::vector3f& sphereCenter, float sphereRadius) const
{
// arvo's algorithm from gamasutra
// http://www.gamasutra.com/features/19991018/Gomez_4.htm

	float s, d = 0.0f; 
//find the square of the distance
//from the sphere to the box
	const gti::vector3f& vmin = center - halfsize;
	const gti::vector3f& vmax = center + halfsize;
	for(unsigned int i=0;i<3;i++) 
	{ 
		if( sphereCenter[i] < vmin[i] )
		{
			s = sphereCenter[i] - vmin[i];
			d += s*s; 
		}
		else if( sphereCenter[i] > vmax[i] )
		{ 
			s = sphereCenter[i] - vmax[i];
			d += s*s; 
		}
	}
	//return d <= r*r

	const float radiusSquared = sphereRadius*sphereRadius;
	if (d <= radiusSquared)
	{
		// make an aabb aabb test with the sphere aabb to test inside state
		AABB sphereAABB;
		sphereAABB.center = sphereCenter;
		sphereAABB.halfsize.set(sphereRadius,sphereRadius,sphereRadius);
		if (overlap(sphereAABB) == INSIDE)
			return INSIDE;
		return OVERLAP;	
	}

	return OUTSIDE;
}